#include "stm32f10x.h"                  	// Device header
#include "sys.h"

float pitch,roll,yaw;				
float zhongzhi=-0.5;//-3;					//机械中值
float measure,theoretic;
int16_t velocity;
int16_t PWM,PWM1,PWM2;

int16_t Encoder1,Encoder2;					//左右编码器
int16_t right_Speed,left_Speed; 			//左右轮速度
static int16_t Speed=0;						//前进后退的速度

int16_t Distance=0;							//接收返回距离数据
int8_t car_mode=0;//平衡车模式 0正常模式（蓝牙遥控加避障检测） -1跟随模式（保持30cm距离）
uint16_t bizhang_count=0;						//进入中断避障次数 100ms读取一次超声波

void Spped_limit(int16_t Speed_limit,int16_t limit)	//前进后退限幅函数
{
	if(Speed>=limit){Speed=limit;}
	if(Speed<=-limit){Speed=-limit;}
}

void car_normal(void)										//平衡车默认模式
{
		mpu_dmp_get_data(&pitch,&roll,&yaw);				//读取姿态信息(其中偏航角有飘移是正常现象)
		measure=roll;										//翻滚角赋值给测量值
		theoretic=zhongzhi;									//机械中值=理论值
//		Encoder1=Get_Encoder_Speed1();						//读取编码器数
//		Encoder2=Get_Encoder_Speed2();						//读取编码器数			
		if((go==0)&&(back==0)&&car_mode==0)					//如果前进后退都没按下并且避障不开启
		{
			right_Speed=0,left_Speed=0;						
			if(Speed>0)										//速度线性减速
				{											//如果pid调参调的好也可以不用线性减速
				Speed-=25;
				if(Speed<=0)
				{
					Speed=0;
				}
			}
			if(Speed<0)
			{
				Speed+=25;
				if(Speed>=0)
				{
					Speed=0;
				}
			}		
		}	
		
		if(go==1&&car_mode==0)											//前进按下 并且避障不开启			
		{
		Speed-=40;
		Spped_limit(Speed,10000);										//速度限幅
		}
		if(back==1&&car_mode==0)											//后退
		{
		Speed+=40;
		Spped_limit(Speed,10000);										//速度限幅
		}
		if(right==1){left_Speed+=700;}							//右转
		if(left==1){right_Speed+=700;}							//左转
		if(brake==1){Speed=0,right_Speed=0,left_Speed=0;}		//急刹，速度都清零
		
		
		velocity=(Get_Encoder_Speed1()+Get_Encoder_Speed2())/2;	//速度	
		if(right==1|left==1&&car_mode==0)									//如果按下了左转或者右转
		{
				PWM=Vertical_PID_value(measure,theoretic)+velocity_PID_value(velocity);
				PWM1=PWM+Speed+right_Speed;
				PWM2=PWM+Speed+left_Speed;	
				Motor__right_SetDoubleSpeed(7200,&PWM1);
				Motor__left_SetDoubleSpeed(7200,&PWM2);
		}
		else if(right==0|left==0&&car_mode==0)								//如果左转或者右转没按下
		{
				PWM=Vertical_PID_value(measure,theoretic)+velocity_PID_value(velocity)+Trun_PID_value(yaw,0);		
				PWM1=PWM+Speed;
				PWM2=PWM+Speed;	
				Motor__right_SetDoubleSpeed(7200,&PWM1);
				Motor__left_SetDoubleSpeed(7200,&PWM2);	
		}
		
		else if(car_mode==-1)								//如果避障开启
		{
			bizhang_count++;
		if(bizhang_count==10)								//10ms进入一次中断 100ms触发一次超声波
		{
			bizhang_count=0;
			Distance=Get_Distance();
			if(Distance<250)								//如果距离小于在25cm
			{												//Speed+=10线性提速 后退
				Speed+=50;								
				Spped_limit(Speed,10000);					//速度限幅
			}
			else if(Distance>250)							//如果大于25cm
			{												//Speed——=10线性提速 前进
				Speed-=50;
				Spped_limit(Speed,10000);
			}
			else if(Distance==250){Speed=0;}				//如果等于30cm不用提速保持距离
		}
		
		PWM=Vertical_PID_value(measure,theoretic)+velocity_PID_value(velocity)+Trun_PID_value(yaw,0);		
		PWM1=PWM+Speed;
		PWM2=PWM+Speed;	
		Motor__right_SetDoubleSpeed(7200,&PWM1);
		Motor__left_SetDoubleSpeed(7200,&PWM2);	
		printf("%d\n",Distance);					//串口发送距离
		}
/*安全设置*/		
		while(roll>65|roll<-65)//如果超过65度则关闭电机
		{
			mpu_dmp_get_data(&pitch,&roll,&yaw);			//读取姿态信息(其中偏航角有飘移是正常现象)
			Motor_close();
		}				
//		Motor_SetDoubleSpeed(7200,&PWM1,&PWM2);
}

int main(void)
{
	/*模块初始化*/
	NVIC_Config();
	OLED_Init();		//OLED初始化
	mpu_dmp_init();
	usart3_init(9600);
	MPU6050_EXTI_Init();
	Motor_Init();
	Encoder_Init();
	TIM3_Init();
	HCSR04_GPIO_Init();
/*直立环参数*/	
	zl_Kp=-450;//1500//1000//700//-360;//-600;
	zl_Kd=-3200;//-2820;//4700//4300//4000//3700//-3500;//-2800;
	/*速度环参数*/	
	sd_Kp=380;//340;//270;////260;//240;
	sd_Ki=1.9;//1.7;	
/*转向环参数*/
	zx_Kp=0;
	zx_Kd=-900;
	
	OLED_ShowString(1,1,"Pitch:");		
	OLED_ShowString(1,1,"YAW:");
	OLED_ShowString(2,1,"ROLL:");
	OLED_ShowString(3,1,"DIST:");
	OLED_ShowString(3,12,"mm");
	OLED_ShowString(4,1,"mode:");
	while (1)
	{
//		Distance=Get_Distance();		
		OLED_ShowSignedNum(1, 8, yaw, 4);//偏航				
		OLED_ShowSignedNum(2, 8,roll, 4);//横滚 平衡车平衡关键角 机械中值确定角
		OLED_ShowSignedNum(3,8,Distance,3);
		OLED_ShowSignedNum(4,8,car_mode,2);
//		Delay_ms(50);
//		printf("%d,%d,%d\n",(int16_t)pitch,(int16_t)yaw,(int16_t)roll);	//串口打印欧拉角
//		printf("AT\n");//串口打印测试
//		printf("%d\n",car_mode);
	}
}



//MPU6050中断
void EXTI9_5_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line5)==SET)
	{	
			car_normal();
		EXTI_ClearITPendingBit(EXTI_Line5);
	}
}



